#include "FollowTrackState.h"
#include "AStarSearch.h"
#include "470bot.h"
#include "PDController.h"
#include "Track.h"

#include <time.h>

FollowTrackState::FollowTrackState(Graph* g, tank_t* tank, std::vector<Vector2> track, bool vertical)
{
	srand ( time(NULL) );

	w = World::instance();
	com = Remote::instance();
	_complete = false;

	this->currentTrack = track;
	
	pos = 0;
	trackPos = 0;

	//currentTrack = track;
	this->g = g;
	this->goal = g->getNode(currentTrack[trackPos]);

	cutoff = 15;
	recalc = false;
	reverseLeft = false;

	cycles = 0;
	lastPos = Vector2(tank->pos[0], tank->pos[1]);
	backup = false;

	cleanup = false;

	isdone = false;
	if(!getDirtyPath(Vector2(tank->pos[0], tank->pos[1]))){
		_complete = true;
		recalc = true;
	}

	vert = vertical;

	stuck = 0;
	cleanupCount=15;
}


FollowTrackState::~FollowTrackState(void)
{
}

Command FollowTrackState::GetCommand(GridAgent* agent){
	bool wait = false;
	
	if(isdone){
		return Command(agent->getTank()->index, 1, 1, false);
	}
	
	if(recalc){
		recalc = false;
		if(_complete){
			_complete = false;
			
			if(cleanup){
				if(World::instance()->getTrack()->isReady()){
					cleanupCount ++;
					if(cleanupCount > 30){
						cleanupCount = 0;
						if(!getMissedPoint(Vector2(agent->getTank()->pos[0], agent->getTank()->pos[1]))){
							isdone =  true;
							World::instance()->getTrack()->markDone();
							return Command(agent->getTank()->index, 0, 0, false);
						}
					}
					else{
						wait = true;
					}
				}
			}
			else if(trackPos == currentTrack.size()-1){
				Track* t = World::instance()->getTrack();
				if(t->getNextTrack(&currentTrack, vert)){
					trackPos = 0;
				}
				else{
					World::instance()->getTrack()->markReady();
					cleanup = true;
					cutoff = 8;
					//isdone =  true;
					//return Command(agent->getTank()->index, 0, 0, false);
				}
			}
			else{
				trackPos++;
			}
			node* goalNode = g->getNode(currentTrack[trackPos]);
			this->goal = goalNode;
		}
		if(!getDirtyPath(Vector2(agent->getTank()->pos[0], agent->getTank()->pos[1]))){
			_complete = true;
			recalc = true;

			return Command(agent->getTank()->index, 0, 0, false);
		}
	}

	if(wait)
		return Command(agent->getTank()->index, 0, 0, false);

	if(backup || cycles >= 20){
		if(cycles >= 20){
			backup = true;
			stuck ++;
			if(stuck >= 5)
				recalc = true;
			reverseLeft = !reverseLeft;
		}
		cycles -= 1;
		if(cycles <= 0){
			backup = false;
		}
		if(reverseLeft)
			return Command(agent->getTank()->index, -1, -1, false);
		else
			return Command(agent->getTank()->index, -1, 1, false);
	}
	
	if(agent->getTank()->pos[0] == lastPos.x && agent->getTank()->pos[1] == lastPos.y){
		cycles++;
	}
	else{
		cycles = 0;
		lastPos = Vector2(agent->getTank()->pos[0], agent->getTank()->pos[1]);
	}

	Vector2 trajectory = pfFieldPath(agent->getTank());

	Vector2 random(rand()%10, rand()%10);

	if(random.magnitude() != 0)
		random = random.normalize();

	trajectory = trajectory + random * .2;

	return PDController::instance()->GetComand(agent->getTank(), trajectory);
}

bool FollowTrackState::done(){
	return isdone;
}

bool FollowTrackState::complete(){
	return true;
}

bool FollowTrackState::getDirtyPath(Vector2 start){
	AStarSearch as;
	
	g->clearSearchFlags();
	if(goal->occupied) return false;

	node* current = as.search(g, start, Vector2(goal->x, goal->y), true);
	
	if(current == NULL)
		return false;

	if(goal->x == current->x && goal->y == current->y) 
		_complete = true;
	
	path.clear();
	path.push_back(Vector2(current->x, current->y)); 
	while(current != NULL){
		path.push_back(Vector2(current->x, current->y));
		current = current->previous;
	}
	path.push_back(start);

	pos = path.size() - 2;

	return true;
}

bool FollowTrackState::getMissedPoint(Vector2 start){
	Vector2 tmp;
	
	if(!World::instance()->getTrack()->initialized())
		World::instance()->getTrack()->initialize();

	currentTrack.clear();

	if(World::instance()->getTrack()->getNextMissedNode(&tmp, start)){
		currentTrack.push_back(tmp);
		trackPos = 0;

		if(!getDirtyPath(start)){
			recalc = true;
			_complete = true;
		}

		return true;
	}
	
	isdone = true;

	return false;
}

Vector2 FollowTrackState::pfFieldPath(tank_t* tank){
	Vector2 field(path[pos].x - tank->pos[0], path[pos].y - tank->pos[1]);
	Vector2 dir;

	//int tmpPos = pos;
	//if(pos > path.size() - 2) tmpPos = path.size() - 2;
	//if(pos < 1) tmpPos = 1;

	dir.x = path[pos].x-path[pos+1].x, dir.y = path[pos].y-path[pos+1].y;

	float dist = field.magnitude();
	float dot = Vector2::dot(dir, field);
	float angle = fabs(dir.angle() - field.angle());

	if(field.magnitude() <= cutoff || 
		(field.magnitude() > cutoff && 
		fabs(dir.angle() - field.angle()) > 45.0f*M_PI/180.0f && 
		Vector2::dot(dir.normalize(), field.normalize()) <= DOT_THRESHOLD)){

		if(pos == 0){
			recalc = true;
		}

		setNextWayPoint(tank);
		field = Vector2(path[pos].x - tank->pos[0], path[pos].y - tank->pos[1]);
	}

	return field;
}

void FollowTrackState::setNextWayPoint(tank_t* tank){
	if(pos == 0)
		return;
	
	float threshold = 1.0f * M_PI / 180.0f;
	int maxSteps = 30;
	int count = 0;

	float error = 0;
	float currentAngle;
	int tmpPos;
	
	Vector2 startPoint = path[pos];
	Vector2 currentPoint;
	Vector2 result;

	tmpPos = pos - 1;

	currentAngle = (path[tmpPos]-startPoint).angle();

	while(error < threshold && tmpPos > 0 && tmpPos < path.size() && count < maxSteps){
		tmpPos--;

		currentPoint = path[tmpPos];
		result = currentPoint - startPoint;
		error = fabs(currentAngle - result.angle());
		count ++;
	}

	//cutoff = max(min(PATH_THRESHOLD, count * 4.0), MIN_PATH_THRESHOLD);

	pos = tmpPos ++;
}